using BepuUtilities;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using static BepuUtilities.GatherScatter;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints
{
    /// <summary>
    /// 将实体B上的点约束在附着到实体A的直线上。
    /// </summary>
    public struct PointOnLineServo : ITwoBodyConstraintDescription<PointOnLineServo>
    {
        /// <summary>
        /// 从主体A的中心到其附着点的局部偏移。
        /// </summary>
        public Vector3 LocalOffsetA;
        /// <summary>
        /// 从主体B的中心到其附着点的局部偏移。
        /// </summary>
        public Vector3 LocalOffsetB;
        /// <summary>
        /// 身体A的局部空间中的线的方向。
        /// </summary>
        public Vector3 LocalDirection;
        /// <summary>
        /// 伺服控制参数。
        /// </summary>
        public ServoSettings ServoSettings;
        /// <summary>
        /// 弹簧频率和阻尼参数。
        /// </summary>
        public SpringSettings SpringSettings;

        public int ConstraintTypeId
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return PointOnLineServoTypeProcessor.BatchTypeId;
            }
        }

        public Type TypeProcessorType => typeof(PointOnLineServoTypeProcessor);

        public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
        {
            ConstraintChecker.AssertUnitLength(LocalDirection, nameof(PointOnLineServo), nameof(LocalDirection));
            ConstraintChecker.AssertValid(ServoSettings, SpringSettings, nameof(PointOnLineServo));
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var target = ref GetOffsetInstance(ref Buffer<PointOnLineServoPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            Vector3Wide.WriteFirst(LocalOffsetA, ref target.LocalOffsetA);
            Vector3Wide.WriteFirst(LocalOffsetB, ref target.LocalOffsetB);
            Vector3Wide.WriteFirst(LocalDirection, ref target.LocalDirection);
            ServoSettingsWide.WriteFirst(ServoSettings, ref target.ServoSettings);
            SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings);
        }

        public void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out PointOnLineServo description)
        {
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var source = ref GetOffsetInstance(ref Buffer<PointOnLineServoPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            Vector3Wide.ReadFirst(source.LocalOffsetA, out description.LocalOffsetA);
            Vector3Wide.ReadFirst(source.LocalOffsetB, out description.LocalOffsetB);
            Vector3Wide.ReadFirst(source.LocalDirection, out description.LocalDirection);
            ServoSettingsWide.ReadFirst(source.ServoSettings, out description.ServoSettings);
            SpringSettingsWide.ReadFirst(source.SpringSettings, out description.SpringSettings);
        }
    }

    public struct PointOnLineServoPrestepData
    {
        public Vector3Wide LocalOffsetA;
        public Vector3Wide LocalOffsetB;
        public Vector3Wide LocalDirection;
        public ServoSettingsWide ServoSettings;
        public SpringSettingsWide SpringSettings;
    }

    public struct PointOnLineServoProjection
    {
        public Matrix2x3Wide LinearJacobian;
        public Vector3Wide OffsetA;
        public Vector3Wide OffsetB;
        public Vector2Wide BiasVelocity;
        public Symmetric2x2Wide EffectiveMass;
        public Vector<float> SoftnessImpulseScale;
        public Vector<float> MaximumImpulse;
        public BodyInertias InertiaA;
        public BodyInertias InertiaB;
    }

    public struct PointOnLineServoFunctions : IConstraintFunctions<PointOnLineServoPrestepData, PointOnLineServoProjection, Vector2Wide>
    {
        static void GetAngularJacobians(in Matrix2x3Wide linearJacobians, in Vector3Wide offsetA, in Vector3Wide offsetB, out Matrix2x3Wide angularJacobianA, out Matrix2x3Wide angularJacobianB)
        {
            Vector3Wide.CrossWithoutOverlap(offsetA, linearJacobians.X, out angularJacobianA.X);
            Vector3Wide.CrossWithoutOverlap(offsetA, linearJacobians.Y, out angularJacobianA.Y);
            Vector3Wide.CrossWithoutOverlap(linearJacobians.X, offsetB, out angularJacobianB.X);
            Vector3Wide.CrossWithoutOverlap(linearJacobians.Y, offsetB, out angularJacobianB.Y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
            ref PointOnLineServoPrestepData prestep, out PointOnLineServoProjection projection)
        {
            // 这会将B上的一个点约束到附着到A的一条直线上。它同时工作在两个自由度上;这两个自由度是与直线方向相切的轴。
            // 误差测量为距直线最近的偏移量。换句话说,就是：
            // Dot(closestPointOnLineToAnclB-AnchorB,T1)=0
            // Dot(closestPointOnLineToAnclB-AnchorB,T2)=0
            // 其中closestPointOnLineToAnchora B=dot(angorB-Anchora,lineDirection)*lineDirection+Anchora
            // 出于此推导的目的,我们将t1、t2和lineDirection视为关于时间的常量。
            // 在下文中,从A的中心到闭合的PointOnLineToAnclB的偏移A,而偏移B指的是LocalOffsetB*方向B。
            // 点(位置A+偏移A-(位置B+偏移B),T1)=0
            // 点(位置A+偏移量A-(位置B+偏移量B),T2)=0
            // T1的速度约束：
            // 点(d/dt(位置A+偏移量A-(位置B+偏移量B),T1)+点(位置A+偏移量A-(位置B+偏移量B),d/dt(T1))=0
            // 将d/dt(T1)视为常量：
            // 点(d/dt(位置A+偏移A-(位置B+偏移B),t1)=0
            // 点(线性A+角度A x偏移量A-线性B角度B x偏移量B,T1)=0
            // 点(线性A,T1)+点(角度A x偏移量A,T1)+点(线性B,-T1)+点(偏移量B x角度B,T1)=0
            // 点(线性A,T1)+点(偏移A x T1,角度A)+点(线性B,-T1)+点(T1 x偏移B,角度B)=0
            // 遵循与第二自由度相同的模式,雅克比是：
            // 线性A：T1、T2
            // 角度A：偏移A x T1,偏移A x T2
            // 线性B：-T1、-T2
            // 角度B：T1 x偏移B,T2 x偏移B

            // 存储选项：
            // 1)从方向和两个偏移重建。仍然存储有效质量张量和惯性张量(3+14标量),但所有Jacobian只需要9个标量。
            // 2)存储T1、T2和两个偏移量。节省了重建ALU成本,但相对于#1增加了3个标量的存储成本。
            // 3)存储JT*Me和J*i^-1。JT*Me需要3*6标量,J*i^-1需要3*8标量。
            // 内存带宽是首要目标,因此#1很有吸引力。但是,除非根据世界方向重新计算切线基数,否则重新计算切线基数是不可靠的
            // 存储附加信息是因为单一方向没有唯一的基础。当在当地方向上建造基础时,这不是一个很重要的问题
            // 因为预计它不会经常变化。
            // 取而代之的是,我们将选择2号。
            // #3将是单核上最快的,因为它需要的ALU工作要少得多,但它需要更多的内存带宽。

            bodies.GatherPose(ref bodyReferences, count, out var ab, out var orientationA, out var orientationB);
            Matrix3x3Wide.CreateFromQuaternion(orientationA, out var orientationMatrixA);
            Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalOffsetA, orientationMatrixA, out var anchorA);
            QuaternionWide.TransformWithoutOverlap(prestep.LocalOffsetB, orientationB, out projection.OffsetB);

            // 通过计算线上距离锚点B最近的点来找到偏移量A。
            Matrix3x3Wide.TransformWithoutOverlap(prestep.LocalDirection, orientationMatrixA, out var direction);
            Vector3Wide.Add(projection.OffsetB, ab, out var anchorB);
            Vector3Wide.Subtract(anchorB, anchorA, out var anchorOffset);
            Vector3Wide.Dot(anchorOffset, direction, out var d);
            Vector3Wide.Scale(direction, d, out var lineStartToClosestPointOnLine);
            Vector3Wide.Add(lineStartToClosestPointOnLine, anchorA, out projection.OffsetA);

            // 再次注意,基础是在局部空间中创建的,以避免快速变化的雅克比。
            Helpers.BuildOrthnormalBasis(prestep.LocalDirection, out var localTangentX, out var localTangentY);
            Matrix3x3Wide.TransformWithoutOverlap(localTangentX, orientationMatrixA, out projection.LinearJacobian.X);
            Matrix3x3Wide.TransformWithoutOverlap(localTangentY, orientationMatrixA, out projection.LinearJacobian.Y);
            GetAngularJacobians(projection.LinearJacobian, projection.OffsetA, projection.OffsetB, out var angularJacobianA, out var angularJacobianB);
            Symmetric2x2Wide.SandwichScale(projection.LinearJacobian, inertiaA.InverseMass + inertiaB.InverseMass, out var linearContribution);
            Symmetric3x3Wide.MatrixSandwich(angularJacobianA, inertiaA.InverseInertiaTensor, out var angularContributionA);
            Symmetric3x3Wide.MatrixSandwich(angularJacobianB, inertiaB.InverseInertiaTensor, out var angularContributionB);
            Symmetric2x2Wide.Add(angularContributionA, angularContributionB, out var inverseEffectiveMass);
            Symmetric2x2Wide.Add(inverseEffectiveMass, linearContribution, out inverseEffectiveMass);

            Symmetric2x2Wide.InvertWithoutOverlap(inverseEffectiveMass, out projection.EffectiveMass);
            projection.InertiaA = inertiaA;
            projection.InertiaB = inertiaB;

            SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
            Symmetric2x2Wide.Scale(projection.EffectiveMass, effectiveMassCFMScale, out projection.EffectiveMass);

            // 计算位置误差和偏置速度。计算误差时请注意减法的顺序-我们希望偏置速度抵消分离。
            Vector2Wide error;
            Vector3Wide.Dot(anchorOffset, projection.LinearJacobian.X, out error.X);
            Vector3Wide.Dot(anchorOffset, projection.LinearJacobian.Y, out error.Y);
            ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out projection.BiasVelocity, out projection.MaximumImpulse);

        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB,
            in Matrix2x3Wide linearJacobian, in Matrix2x3Wide angularJacobianA, in Matrix2x3Wide angularJacobianB, in BodyInertias inertiaA, in BodyInertias inertiaB, ref Vector2Wide csi)
        {
            Matrix2x3Wide.Transform(csi, linearJacobian, out var linearImpulseA);
            Matrix2x3Wide.Transform(csi, angularJacobianA, out var angularImpulseA);
            Matrix2x3Wide.Transform(csi, angularJacobianB, out var angularImpulseB);
            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, inertiaA.InverseInertiaTensor, out var angularChangeA);
            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseB, inertiaB.InverseInertiaTensor, out var angularChangeB);
            Vector3Wide.Scale(linearImpulseA, inertiaA.InverseMass, out var linearChangeA);
            Vector3Wide.Scale(linearImpulseA, inertiaB.InverseMass, out var negatedLinearChangeB);

            Vector3Wide.Add(linearChangeA, velocityA.Linear, out velocityA.Linear);
            Vector3Wide.Add(angularChangeA, velocityA.Angular, out velocityA.Angular);
            Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear);
            Vector3Wide.Add(angularChangeB, velocityB.Angular, out velocityB.Angular);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref PointOnLineServoProjection projection, ref Vector2Wide accumulatedImpulse)
        {
            GetAngularJacobians(projection.LinearJacobian, projection.OffsetA, projection.OffsetB, out var angularA, out var angularB);
            ApplyImpulse(ref velocityA, ref velocityB, projection.LinearJacobian, angularA, angularB, projection.InertiaA, projection.InertiaB, ref accumulatedImpulse);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref PointOnLineServoProjection projection, ref Vector2Wide accumulatedImpulse)
        {
            // CSI=投射.BiasImPulse-累加脉冲*投射.SoftnessImPulseScale-(csia线性+csiaAngular+csib线性+csibAngular);
            GetAngularJacobians(projection.LinearJacobian, projection.OffsetA, projection.OffsetB, out var angularA, out var angularB);
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityA.Linear, projection.LinearJacobian, out var linearCSVA);
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityB.Linear, projection.LinearJacobian, out var negatedLinearCSVB);
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityA.Angular, angularA, out var angularCSVA);
            Matrix2x3Wide.TransformByTransposeWithoutOverlap(velocityB.Angular, angularB, out var angularCSVB);
            Vector2Wide.Subtract(linearCSVA, negatedLinearCSVB, out var linearCSV);
            Vector2Wide.Add(angularCSVA, angularCSVB, out var angularCSV);
            Vector2Wide.Add(linearCSV, angularCSV, out var csv);
            Vector2Wide.Subtract(projection.BiasVelocity, csv, out csv);
            Symmetric2x2Wide.TransformWithoutOverlap(csv, projection.EffectiveMass, out var csi);
            Vector2Wide.Scale(accumulatedImpulse, projection.SoftnessImpulseScale, out var softnessContribution);
            Vector2Wide.Subtract(csi, softnessContribution, out csi);
            ServoSettingsWide.ClampImpulse(projection.MaximumImpulse, ref accumulatedImpulse, ref csi);
            ApplyImpulse(ref velocityA, ref velocityB, projection.LinearJacobian, angularA, angularB, projection.InertiaA, projection.InertiaB, ref csi);
        }

    }

    public class PointOnLineServoTypeProcessor : TwoBodyTypeProcessor<PointOnLineServoPrestepData, PointOnLineServoProjection, Vector2Wide, PointOnLineServoFunctions>
    {
        public const int BatchTypeId = 37;
    }
}
